//
// Created by az on 2020/6/25.
//


#include "env_edt/corridor_generator.h"

void CORRIDOR_GENERATOR::init(shared_ptr<sdf_tools::CollisionMapGrid>& _map, vector<double> map_size, ros::NodeHandle &nh) {
    nh.param("planning/max_inflate",   max_inflate_iter_, 100);
    nh.param("planning/step_length",   step_length_,      2);
    nh.param("planning/cube_margin",   cube_margin_,      0.2);

    is_init_ = true;
    map_size_.swap(map_size);

    collision_map_    = _map;
    resolution_       = collision_map_->GetResolution();

    max_x_id_ = (int)(collision_map_->GetXSize()/resolution_);
    max_y_id_ = (int)(collision_map_->GetYSize()/resolution_);
    max_z_id_ = (int)(collision_map_->GetZSize()/resolution_);
}

pair<Cube, bool> CORRIDOR_GENERATOR::inflateCube(Cube cube, Cube lstcube)
{
    Cube cubeMax = cube;

    // Inflate sequence: left, right, front, back, below, above
    Eigen::MatrixXi vertex_idx(8, 3);
    for (int i = 0; i < 8; i++)
    {
        double coord_x = max(min(cube.vertex(i, 0), map_size_[1]), map_size_[0]);
        double coord_y = max(min(cube.vertex(i, 1), map_size_[3]), map_size_[2]);
        double coord_z = max(min(cube.vertex(i, 2), resolution_), 0.0);
        Eigen::Vector3d coord(coord_x, coord_y, coord_z);

        Eigen::Vector3i pt_idx = collision_map_->LocationToGridIndex(coord);

        if( collision_map_->Get( (int64_t)pt_idx(0), (int64_t)pt_idx(1), (int64_t)pt_idx(2) ).first.occupancy > 0.5 )
        {
            ROS_ERROR("[Planning Node] path has node in obstacles !");
            return make_pair(cubeMax, false);
        }

        vertex_idx.row(i) = pt_idx;
    }

    int id_x, id_y, id_z;

    /*
               P4------------P3
               /|           /|              ^
              / |          / |              | z
            P1--|---------P2 |              |
             |  P8--------|--p7             |
             | /          | /               /--------> y
             |/           |/               /
            P5------------P6              / x
    */

    // Y- now is the left side : (p1 -- p4 -- p8 -- p5) face sweep
    // ############################################################################################################
    bool collide;

    Eigen::MatrixXi vertex_idx_lst = vertex_idx;

    int iter = 0;
    while(iter < max_inflate_iter_)
    {
        collide  = false;
        int y_lo = max(0, vertex_idx(0, 1) - step_length_);
        int y_up = min(max_y_id_, vertex_idx(1, 1) + step_length_);

        for(id_y = vertex_idx(0, 1); id_y >= y_lo; id_y-- )
        {
            if( collide == true)
                break;

            for(id_x = vertex_idx(0, 0); id_x >= vertex_idx(3, 0); id_x-- )
            {
                if( collide == true)
                    break;

                for(id_z = vertex_idx(0, 2); id_z >= vertex_idx(4, 2); id_z-- )
                {
                    double occupy = collision_map_->Get( (int64_t)id_x, (int64_t)id_y, (int64_t)id_z).first.occupancy;
                    if(occupy > 0.5) // the voxel is occupied
                    {
                        collide = true;
                        break;
                    }
                }
            }
        }

        if(collide)
        {
            vertex_idx(0, 1) = min(id_y+2, vertex_idx(0, 1));
            vertex_idx(3, 1) = min(id_y+2, vertex_idx(3, 1));
            vertex_idx(7, 1) = min(id_y+2, vertex_idx(7, 1));
            vertex_idx(4, 1) = min(id_y+2, vertex_idx(4, 1));
        }
        else
            vertex_idx(0, 1) = vertex_idx(3, 1) = vertex_idx(7, 1) = vertex_idx(4, 1) = id_y + 1;

        // Y+ now is the right side : (p2 -- p3 -- p7 -- p6) face
        // ############################################################################################################
        collide = false;
        for(id_y = vertex_idx(1, 1); id_y <= y_up; id_y++ )
        {
            if( collide == true)
                break;

            for(id_x = vertex_idx(1, 0); id_x >= vertex_idx(2, 0); id_x-- )
            {
                if( collide == true)
                    break;

                for(id_z = vertex_idx(1, 2); id_z >= vertex_idx(5, 2); id_z-- )
                {
                    double occupy = collision_map_->Get( (int64_t)id_x, (int64_t)id_y, (int64_t)id_z).first.occupancy;
                    if(occupy > 0.5) // the voxel is occupied
                    {
                        collide = true;
                        break;
                    }
                }
            }
        }

        if(collide)
        {
            vertex_idx(1, 1) = max(id_y-2, vertex_idx(1, 1));
            vertex_idx(2, 1) = max(id_y-2, vertex_idx(2, 1));
            vertex_idx(6, 1) = max(id_y-2, vertex_idx(6, 1));
            vertex_idx(5, 1) = max(id_y-2, vertex_idx(5, 1));
        }
        else
            vertex_idx(1, 1) = vertex_idx(2, 1) = vertex_idx(6, 1) = vertex_idx(5, 1) = id_y - 1;

        // X + now is the front side : (p1 -- p2 -- p6 -- p5) face
        // ############################################################################################################
        int x_lo = max(0, vertex_idx(3, 0) - step_length_);
        int x_up = min(max_x_id_, vertex_idx(0, 0) + step_length_);

        collide = false;
        for(id_x = vertex_idx(0, 0); id_x <= x_up; id_x++ )
        {
            if( collide == true)
                break;

            for(id_y = vertex_idx(0, 1); id_y <= vertex_idx(1, 1); id_y++ )
            {
                if( collide == true)
                    break;

                for(id_z = vertex_idx(0, 2); id_z >= vertex_idx(4, 2); id_z-- )
                {
                    double occupy = collision_map_->Get( (int64_t)id_x, (int64_t)id_y, (int64_t)id_z).first.occupancy;
                    if(occupy > 0.5) // the voxel is occupied
                    {
                        collide = true;
                        break;
                    }
                }
            }
        }

        if(collide)
        {
            vertex_idx(0, 0) = max(id_x-2, vertex_idx(0, 0));
            vertex_idx(1, 0) = max(id_x-2, vertex_idx(1, 0));
            vertex_idx(5, 0) = max(id_x-2, vertex_idx(5, 0));
            vertex_idx(4, 0) = max(id_x-2, vertex_idx(4, 0));
        }
        else
            vertex_idx(0, 0) = vertex_idx(1, 0) = vertex_idx(5, 0) = vertex_idx(4, 0) = id_x - 1;

        // X- now is the back side : (p4 -- p3 -- p7 -- p8) face
        // ############################################################################################################
        collide = false;
        for(id_x = vertex_idx(3, 0); id_x >= x_lo; id_x-- )
        {
            if( collide == true)
                break;

            for(id_y = vertex_idx(3, 1); id_y <= vertex_idx(2, 1); id_y++ )
            {
                if( collide == true)
                    break;

                for(id_z = vertex_idx(3, 2); id_z >= vertex_idx(7, 2); id_z-- )
                {
                    double occupy = collision_map_->Get( (int64_t)id_x, (int64_t)id_y, (int64_t)id_z).first.occupancy;
                    if(occupy > 0.5) // the voxel is occupied
                    {
                        collide = true;
                        break;
                    }
                }
            }
        }

        if(collide)
        {
            vertex_idx(3, 0) = min(id_x+2, vertex_idx(3, 0));
            vertex_idx(2, 0) = min(id_x+2, vertex_idx(2, 0));
            vertex_idx(6, 0) = min(id_x+2, vertex_idx(6, 0));
            vertex_idx(7, 0) = min(id_x+2, vertex_idx(7, 0));
        }
        else
            vertex_idx(3, 0) = vertex_idx(2, 0) = vertex_idx(6, 0) = vertex_idx(7, 0) = id_x + 1;

        // Z+ now is the above side : (p1 -- p2 -- p3 -- p4) face
        // ############################################################################################################
        collide = false;
        int z_lo = max(0, vertex_idx(4, 2) - step_length_);
        int z_up = min(max_z_id_, vertex_idx(0, 2) + step_length_);
        for(id_z = vertex_idx(0, 2); id_z <= z_up; id_z++ )
        {
            if( collide == true)
                break;

            for(id_y = vertex_idx(0, 1); id_y <= vertex_idx(1, 1); id_y++ )
            {
                if( collide == true)
                    break;

                for(id_x = vertex_idx(0, 0); id_x >= vertex_idx(3, 0); id_x-- )
                {
                    double occupy = collision_map_->Get( (int64_t)id_x, (int64_t)id_y, (int64_t)id_z).first.occupancy;
                    if(occupy > 0.5) // the voxel is occupied
                    {
                        collide = true;
                        break;
                    }
                }
            }
        }

        if(collide)
        {
            vertex_idx(0, 2) = max(id_z-2, vertex_idx(0, 2));
            vertex_idx(1, 2) = max(id_z-2, vertex_idx(1, 2));
            vertex_idx(2, 2) = max(id_z-2, vertex_idx(2, 2));
            vertex_idx(3, 2) = max(id_z-2, vertex_idx(3, 2));
        }
        vertex_idx(0, 2) = vertex_idx(1, 2) = vertex_idx(2, 2) = vertex_idx(3, 2) = id_z - 1;

        // now is the below side : (p5 -- p6 -- p7 -- p8) face
        // ############################################################################################################
        collide = false;
        for(id_z = vertex_idx(4, 2); id_z >= z_lo; id_z-- )
        {
            if( collide == true)
                break;

            for(id_y = vertex_idx(4, 1); id_y <= vertex_idx(5, 1); id_y++ )
            {
                if( collide == true)
                    break;

                for(id_x = vertex_idx(4, 0); id_x >= vertex_idx(7, 0); id_x-- )
                {
                    double occupy = collision_map_->Get( (int64_t)id_x, (int64_t)id_y, (int64_t)id_z).first.occupancy;
                    if(occupy > 0.5) // the voxel is occupied
                    {
                        collide = true;
                        break;
                    }
                }
            }
        }

        if(collide)
        {
            vertex_idx(4, 2) = min(id_z+2, vertex_idx(4, 2));
            vertex_idx(5, 2) = min(id_z+2, vertex_idx(5, 2));
            vertex_idx(6, 2) = min(id_z+2, vertex_idx(6, 2));
            vertex_idx(7, 2) = min(id_z+2, vertex_idx(7, 2));
        }
        else
            vertex_idx(4, 2) = vertex_idx(5, 2) = vertex_idx(6, 2) = vertex_idx(7, 2) = id_z + 1;

        if(vertex_idx_lst == vertex_idx)
            break;

        vertex_idx_lst = vertex_idx;

        Eigen::MatrixXd vertex_coord(8, 3);
        for(int i = 0; i < 8; i++)
        {
            int index_x = max(min(vertex_idx(i, 0), max_x_id_ - 1), 0);
            int index_y = max(min(vertex_idx(i, 1), max_y_id_ - 1), 0);
            int index_z = max(min(vertex_idx(i, 2), max_z_id_ - 1), 0);

            Eigen::Vector3i index(index_x, index_y, index_z);
            Eigen::Vector3d pos = collision_map_->GridIndexToLocation(index);
            vertex_coord.row(i) = pos;
        }

        cubeMax.setVertex(vertex_coord, resolution_);
        if( isContains(lstcube, cubeMax))
            return make_pair(lstcube, false);

        iter ++;
    }

    return make_pair(cubeMax, true);
}

Cube CORRIDOR_GENERATOR::
generateCube( Eigen::Vector3d pt){
/*
           P4------------P3
           /|           /|              ^
          / |          / |              | z
        P1--|---------P2 |              |
         |  P8--------|--p7             |
         | /          | /               /--------> y
         |/           |/               /
        P5------------P6              / x
*/
    Cube cube;

    pt(0) = max(min(pt(0), map_size_[1]), map_size_[0]);
    pt(1) = max(min(pt(1), map_size_[3]), map_size_[2]);
    pt(2) = max(min(pt(2), resolution_), 0.0);

    Eigen::Vector3i pc_index = collision_map_->LocationToGridIndex(pt);
    Eigen::Vector3d pc_coord = collision_map_->GridIndexToLocation(pc_index);

    cube.center = pc_coord;
    double x_u = pc_coord(0);
    double x_l = pc_coord(0);

    double y_u = pc_coord(1);
    double y_l = pc_coord(1);

    double z_u = pc_coord(2);
    double z_l = pc_coord(2);

    cube.vertex.row(0) = Eigen::Vector3d(x_u, y_l, z_u);
    cube.vertex.row(1) = Eigen::Vector3d(x_u, y_u, z_u);
    cube.vertex.row(2) = Eigen::Vector3d(x_l, y_u, z_u);
    cube.vertex.row(3) = Eigen::Vector3d(x_l, y_l, z_u);

    cube.vertex.row(4) = Eigen::Vector3d(x_u, y_l, z_l);
    cube.vertex.row(5) = Eigen::Vector3d(x_u, y_u, z_l);
    cube.vertex.row(6) = Eigen::Vector3d(x_l, y_u, z_l);
    cube.vertex.row(7) = Eigen::Vector3d(x_l, y_l, z_l);

    return cube;
}

bool CORRIDOR_GENERATOR::isContains(Cube cube1, Cube cube2)
{
    if( cube1.vertex(0, 0) >= cube2.vertex(0, 0) && cube1.vertex(0, 1) <= cube2.vertex(0, 1) && cube1.vertex(0, 2) >= cube2.vertex(0, 2) &&
        cube1.vertex(6, 0) <= cube2.vertex(6, 0) && cube1.vertex(6, 1) >= cube2.vertex(6, 1) && cube1.vertex(6, 2) <= cube2.vertex(6, 2)  )
        return true;
    else
        return false;
}

void CORRIDOR_GENERATOR::corridorSimplify(vector<Cube> & cubicList)
{
    vector<Cube> cubicSimplifyList;
    for(int j = (int)cubicList.size() - 1; j >= 0; j--)
    {
        for(int k = j - 1; k >= 0; k--)
        {
            if(cubicList[k].valid == false)
                continue;
            else if(isContains(cubicList[j], cubicList[k]))
                cubicList[k].valid = false;
        }
    }

    for(auto cube:cubicList)
        if(cube.valid == true)
            cubicSimplifyList.push_back(cube);

    cubicList = cubicSimplifyList;
}

void CORRIDOR_GENERATOR::corridorGeneration(vector<Cube>& cubeList, vector<Eigen::Vector3d> path_coord, vector<double> time)
{
    if(!is_init_) {
        ROS_WARN("[corridor generator]:product corridor error !");
        return;
    }

    Eigen::Vector3d pt;

    Cube lstcube;

    for (int i = 0; i < (int)path_coord.size(); i += 1)
    {
        pt = path_coord[i];

        Cube cube = generateCube(pt);
        auto result = inflateCube(cube, lstcube);

        if(result.second == false)
            continue;

        cube = result.first;

        lstcube = cube;
        cube.t = time[i];
        cubeList.push_back(cube);
    }
}

void CORRIDOR_GENERATOR::corridorGeneration(vector<Cube>& cubeList, vector<Eigen::Vector3d> path_coord)
{
    if(!is_init_) {
        ROS_WARN("[corridor generator]:product corridor error !");
        return;
    }
    Eigen::Vector3d pt;

    Cube lstcube;

    for (int i = 0; i < (int)path_coord.size(); i += 1)
    {
        pt = path_coord[i];

        Cube cube = generateCube(pt);
        auto result = inflateCube(cube, lstcube);

        if(result.second == false)
            continue;

        cube = result.first;

        lstcube = cube;
        cubeList.push_back(cube);
    }
}
